"""
投放航线规划

date: 2025/8/16
author: SiHeng Tang
file: release_pf.py
copyright(c) DFSA Eastern Software
此程序不能提供任何担保 WITHOUT WARRANTY OF ANY KIND
"""
import logging
import time
from copy import deepcopy

from async_sche import TaskWrapper, BaseTaskGen, ExitGenerationError
from helpers.geo_math_trans import GeoVector
from helpers.release_path import get_path_adviser
from mengji import Vehicle
from mod_loader import ConfigHandler
from task_element import DataEndpoint, DataPack

logger = logging.getLogger(__name__)
handler = ConfigHandler(__name__)

DEFAULT_CONFIG = {
    "label": "release_pf",
    "update_rate_hz": 2,
    "default_enable": 0,
    "adviser_name": "legacy",
    "adviser_params": {}
}


class ReleasePathFinder(BaseTaskGen):
    """
    落点预测模块使用工具函数库中的落点模型接口进行落点计算。
    此模块的预测数据通过端点发送，可以使用 ``drop_point_ep`` 监听，
    端点输出字典::

        {
            "advice_hdg": deg(float),
            "distance_to_target": deg(float),
            "etc_time": deg(float)
        }
    """

    def __init__(self, cfg: dict):
        super().__init__(priority=TaskWrapper.IDLE_PRIORITY + 10)
        self._config = cfg
        self._enable_advice = self._config["default_enable"]

        self.recv_drop_point_ep = DataEndpoint()
        self.recv_gps_ep = DataEndpoint()
        self.pub_release_advice_ep = DataEndpoint()

        self._target_pos_gv = GeoVector(lat=0, lon=0)
        self._pa = get_path_adviser(self._config["adviser_name"], self._config["adviser_params"])

        self._last_pub_time = time.time()

    def _on_gen(self):
        if not self.recv_drop_point_ep.detect_new_in_dp():
            raise ExitGenerationError()

    async def _func(self):
        pos_gv = self.recv_drop_point_ep.get_dp().get_a("payload")
        gps_dict = self.recv_gps_ep.get_dp().get_a("payload")

        if pos_gv:
            self._pa.update_path(pos_gv)

        # 没有 GPS 数据判断为连接断开，不产生有效航向预测数据
        if not (gps_dict and self._enable_advice):
            return

        advice = self._pa.get_next_advice(gps_dict["hdg"])

        # 后剩余时间和剩余距离过小判断为无效值，不更新
        if advice[1] < 1e-2:
            return

        payload = {
            "advice_hdg": advice[0],
            "distance_to_target": advice[1]
        }

        if (time.time() - self._last_pub_time) > (1.0 / self._config["update_rate_hz"]):
            self.pub_release_advice_ep.set_dp(DataPack(payload=payload, gen_time=time.time()))
            self._last_pub_time = time.time()

    def _on_exit(self):
        self.pub_release_advice_ep = None
        self.recv_gps_ep = None
        self.recv_drop_point_ep = None

    ####################
    # 控制接口
    ####################
    def set_enable_path_advice(self, enable=True):
        self._enable_advice = enable

    def set_target_pos(self, lat: float, lon: float):
        self._pa.set_target(GeoVector(lat=lat, lon=lon))


@handler.add_handler("unit_proc:release_pf")
def load_config(veh: Vehicle, cfg_section: dict):
    logger.debug(f"release path finder load config {cfg_section}")

    cfg = deepcopy(DEFAULT_CONFIG)
    cfg.update(cfg_section)

    veh.reg_part(cfg["label"], ReleasePathFinder(cfg), is_sche=True)
